#ifndef VL_BINARY_TREE_HPP
#define VL_BINARY_TREE_HPP

#include <functional>
#include "debug.hpp"
#include <map>

namespace vl
{

struct tree_node_common
{
	tree_node_common()
	: red(false)
	{
	}
	
	bool red;
	tree_node_common* ch[2];
	tree_node_common* parent;
};

template<typename T>
struct tree_data_node : tree_node_common
{
	tree_data_node(T const& data)
	: data(data)
	{
	}
	
	bool operator<(tree_data_node const& b) const
	{
		return data < b.data;
	}
	
	T data;
};

struct binary_tree_common
{
	tree_node_common head;
	tree_node_common* root;
	
	binary_tree_common()
	: root(&head)
	{
		head.ch[0] = &head;
		head.ch[1] = &head;
		head.parent = 0;
	}
	
	template<int Dir>
	void rotate(tree_node_common* v)
	{
		int const OpDir = Dir ^ 1;
		
		tree_node_common* y = v->ch[OpDir];
		
		v->ch[OpDir] = y->ch[Dir];
		if(y->ch[Dir] != &head)
			y->ch[Dir]->parent = v;

		if(y != &head)
			y->parent = v->parent;
		if(v->parent)
		{
			if(v == v->parent->ch[Dir])
				v->parent->ch[Dir] = y;
			else
				v->parent->ch[OpDir] = y;
		}
		else
			root = y;
			
		y->ch[Dir] = v;
		if(v != &head)
		{
			v->parent = y;
			ASSERT(v->parent != &head);
		}
	}
	
	void balance(tree_node_common* v)
	{
		while(v != root && v->parent->red)
		{
			if(v->parent == v->parent->parent->ch[0])
			{
				tree_node_common* y = v->parent->parent->ch[1];
				
				if(y->red)
				{
					v->parent->red = false;
					y->red = false;
					v->parent->parent->red = true;
					v = v->parent->parent;
				}
				else
				{
					if(v == v->parent->ch[1])
					{
						v = v->parent;
						rotate<0>(v);
					}
					
					v->parent->red = false;
					v->parent->parent->red = true;
					rotate<1>(v->parent->parent);
				}
			}
			else
			{
				tree_node_common* y = v->parent->parent->ch[0];
				
				if(y->red)
				{
					v->parent->red = false;
					y->red = false;
					v->parent->parent->red = true;
					v = v->parent->parent;
				}
				else
				{
					if(v == v->parent->ch[0])
					{
						v = v->parent;
						rotate<1>(v);
					}
					
					v->parent->red = false;
					v->parent->parent->red = true;
					rotate<0>(v->parent->parent);
				}
			}
		}
		
		root->red = false;
	}
	
	void check_node(tree_node_common* parent, tree_node_common* v, bool mustBeBlack)
	{
		if(v == &head)
			return;
			
		ASSERT(!mustBeBlack || !v->red);
		ASSERT(v->parent == parent);
		check_node(v, v->ch[0], v->red);
		check_node(v, v->ch[1], v->red);
	}
	
	void check()
	{
		ASSERT(!head.red);
		check_node(0, root, true);
	}
	
	void unlink_all()
	{
		head.ch[0] = &head;
		head.ch[1] = &head;
		head.parent = 0;
		root = &head;
	}
	
#if 0 // TODO
	void unlink(tree_node_common* v)
	{
	
		tree_node_common* fixnode;	// the node to recolor as needed
		tree_node_common* fixnode_parent;	// parent of _Fixnode (which may be nil)
		tree_node_common* pnode = v;

		if (pnode->left == &head)
			fixnode = pnode->left;	// must stitch up right subtree
		else if (pnode->right == &head)
			fixnode = pnode->right;	// must stitch up left subtree
		else
		{
			pnode = v->successor();
			fixnode = pnode->right;	// _Fixnode is its only subtree
		}
		
		if (pnode == v)
		{
			fixnode_parent = v->parent;
			if(fixnode != &head)
				fixnode->parent = fixnode_parent;	// link up

			if (root == v)
				root = fixnode;	// link down from root
			else if (fixnode_parent->left == v)
				fixnode_parent->left = fixnode;	// link down to left
			else
				fixnode_parent->right = fixnode;	// link down to right

			if (leftmost() == v)
				rightmost() = (fixnode == &head)
					? fixnode_parent	// smallest is parent of erased node
					: min(fixnode);	// smallest in relinked subtree

			if (rightmost() == v)
				rightmost() = (fixnode == &head)
					? fixnode_parent	// largest is parent of erased node
					: max(fixnode);	// largest in relinked subtree
			}
		else
			{	// erased has two subtrees, pnode is successor to erased
			v->left->parent = pnode;	// link left up
			pnode->left = v->left;	// link successor down

			if (pnode == v->right)
				fixnode_parent = pnode;	// successor is next to erased
			else
			{	// successor further down, link in place of erased
				fixnode_parent = pnode->parent;	// parent is successor's
				if (fixnode != &head)
					fixnode->parent = fixnode_parent;	// link fix up
				fixnode_parent->left = fixnode;	// link fix down
				pnode->right = _Right(v);	// link successor down
				v->right->parent = pnode;	// link right up
			}
			
			// CONTINUE HERE

			if (root == v)
				root = pnode;	// link down from root
			else if (_Left(_Parent(v)) == v)
				_Left(_Parent(v)) = pnode;	// link down to left
			else
				_Right(_Parent(v)) = pnode;	// link down to right

			_Parent(pnode) = _Parent(v);	// link successor up
			std::swap(_Color(pnode), _Color(v));	// recolor it
			}

		if (_Color(v) == _Black)
			{	// erasing black link, must recolor/rebalance tree
			for (; fixnode != root && _Color(fixnode) == _Black;
				fixnode_parent = _Parent(fixnode))
				if (fixnode == _Left(fixnode_parent))
					{	// fixup left subtree
					pnode = _Right(fixnode_parent);
					if (_Color(pnode) == _Red)
						{	// rotate red up from right subtree
						_Color(pnode) = _Black;
						_Color(fixnode_parent) = _Red;
						_Lrotate(fixnode_parent);
						pnode = _Right(fixnode_parent);
						}

					if (_Isnil(pnode))
						fixnode = fixnode_parent;	// shouldn't happen
					else if (_Color(_Left(pnode)) == _Black
						&& _Color(_Right(pnode)) == _Black)
						{	// redden right subtree with black children
						_Color(pnode) = _Red;
						fixnode = fixnode_parent;
						}
					else
						{	// must rearrange right subtree
						if (_Color(_Right(pnode)) == _Black)
							{	// rotate red up from left sub-subtree
							_Color(_Left(pnode)) = _Black;
							_Color(pnode) = _Red;
							_Rrotate(pnode);
							pnode = _Right(fixnode_parent);
							}

						_Color(pnode) = _Color(fixnode_parent);
						_Color(fixnode_parent) = _Black;
						_Color(_Right(pnode)) = _Black;
						_Lrotate(fixnode_parent);
						break;	// tree now recolored/rebalanced
						}
					}
				else
					{	// fixup right subtree
					pnode = _Left(fixnode_parent);
					if (_Color(pnode) == _Red)
						{	// rotate red up from left subtree
						_Color(pnode) = _Black;
						_Color(fixnode_parent) = _Red;
						_Rrotate(fixnode_parent);
						pnode = _Left(fixnode_parent);
						}
					if (_Isnil(pnode))
						fixnode = fixnode_parent;	// shouldn't happen
					else if (_Color(_Right(pnode)) == _Black
						&& _Color(_Left(pnode)) == _Black)
						{	// redden left subtree with black children
						_Color(pnode) = _Red;
						fixnode = fixnode_parent;
						}
					else
						{	// must rearrange left subtree
						if (_Color(_Left(pnode)) == _Black)
							{	// rotate red up from right sub-subtree
							_Color(_Right(pnode)) = _Black;
							_Color(pnode) = _Red;
							_Lrotate(pnode);
							pnode = _Left(fixnode_parent);
							}

						_Color(pnode) = _Color(fixnode_parent);
						_Color(fixnode_parent) = _Black;
						_Color(_Left(pnode)) = _Black;
						_Rrotate(fixnode_parent);
						break;	// tree now recolored/rebalanced
						}
					}

			_Color(fixnode) = _Black;	// ensure stopping node is black
			}
	}
#endif
};

template<typename T, typename Compare = std::less<T> >
struct binary_tree : binary_tree_common, Compare
{
	binary_tree(Compare const& compare = Compare())
	: Compare(compare)
	{
	}
	
	void insert(T* v)
	{
		tree_node_common* cur = root;
		tree_node_common* parent = 0;
		
		v->ch[0] = &head;
		v->ch[1] = &head;
		v->red = true;
		
		if(cur != &head)
		{
			while(true)
			{
				parent = cur;
				if(Compare::operator()(*v, *static_cast<T*>(cur)))
				{
					cur = cur->ch[0];
					if(cur == &head)
					{
						parent->ch[0] = v;
						v->parent = parent;
						balance(v);
						return;
					}
				}
				else if(Compare::operator()(*static_cast<T*>(cur), *v))
				{
					cur = cur->ch[1];
					if(cur == &head)
					{
						parent->ch[1] = v;
						v->parent = parent;
						balance(v);
						return;
					}
				}
				else
				{
					// What to do here?
					delete v;
					return;
				}
			}
		}
		
		root = v;
		root->parent = 0;
		root->red = false;
	}
	
	template<typename SpecKeyT>
	T* find(SpecKeyT const& v)
	{
		tree_node_common* cur = root;
		
		while(cur != &head)
		{
			if(Compare::operator()(v, *static_cast<T*>(cur)))
				cur = cur->ch[0];
			else if(Compare::operator()(*static_cast<T*>(cur), v))
				cur = cur->ch[1];
			else
				return static_cast<T*>(cur);
		}
		
		return 0;
	}
};

}

#endif // VL_BINARY_TREE_HPP
